/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "Wed_memheap.h"

/*******************************************************************************
 * Code
 ******************************************************************************/
/**
 * \brief 内存堆初始化
 *
 * @param p_mem_heap   要初始化的内存堆结构体
 * @param p_start_addr 内存起始地址
 * @param size         内存大小
 *
 * @return 成功返回 0
 */
int Wed_memheap_init(struct Wed_memheap *p_mem_heap,
                     void               *p_start_addr,
                     unsigned long       size){
    struct Wed_memheap_item *p_item = NULL;

    if ((p_mem_heap == NULL) || (p_start_addr == NULL) || (size == 0)) {
        return -WED_EINVAL;
    }

    p_mem_heap->p_start_addr   = p_start_addr;
    p_mem_heap->mem_size       = WED_ALIGN_DOWN(size, WED_MEMHEAP_ALIGN_SIZE);
    /* 减去两个内存条目结构体的大小，初始结构为 [内存条目结构体|****可用内存****|内存条目结构体] */
    p_mem_heap->available_size = p_mem_heap->mem_size - (2 * WED_MEMHEAP_ITEM_SIZE);
    p_mem_heap->max_used_size  = p_mem_heap->mem_size - p_mem_heap->available_size;

    /* 初始化空闲链表头 */
    p_item              = &(p_mem_heap->free_header);
    p_item->magic       = WED_MEMHEAP_MAGIC;
    p_item->p_memheap   = p_mem_heap;
    p_item->p_next      = NULL;
    p_item->p_prev      = NULL;
    p_item->p_next_free = p_item;
    p_item->p_prev_free = p_item;

    /* 设置空闲链表到空闲链表头部 */
    p_mem_heap->p_free_list = p_item;

    /* 初始化第一个内存块 */
    p_item              = (struct Wed_memheap_item *)p_start_addr;
    p_item->magic       = WED_MEMHEAP_MAGIC;
    p_item->p_memheap   = p_mem_heap;
    p_item->p_next      = NULL;
    p_item->p_prev      = NULL;
    p_item->p_next_free = p_item;
    p_item->p_prev_free = p_item;

    p_item->p_next = (struct Wed_memheap_item *)
                 ((uint8_t *)p_item + p_mem_heap->available_size + WED_MEMHEAP_ITEM_SIZE);
    p_item->p_prev = p_item->p_next;

    /* block list header */
    p_mem_heap->p_block_list = p_item;

    /* 添加第一个内存块到空闲链表中 */
    p_item->p_next_free = p_mem_heap->p_free_list->p_next_free;
    p_item->p_prev_free = p_mem_heap->p_free_list;
    p_mem_heap->p_free_list->p_next_free->p_prev_free = p_item;
    p_mem_heap->p_free_list->p_next_free              = p_item;

    /* 初始化尾部的内存条目结构体 */
    p_item = p_item->p_next;
    /* 这是一个已经使用的内存条目 */
    p_item->magic     = WED_MEMHEAP_MAGIC | WED_MEMHEAP_ITEM_USED;
    p_item->p_memheap = p_mem_heap;
    p_item->p_next    = (struct Wed_memheap_item *)p_start_addr;
    p_item->p_prev    = (struct Wed_memheap_item *)p_start_addr;
    /* 不在空闲内存链表中 */
    p_item->p_next_free = p_item->p_prev_free = NULL;

    /* 初始化互斥锁 */
    return Wed_mutex_init(&p_mem_heap->lock);
}

/**
 * \brief 内存堆内存分配
 *
 * @param p_mem_heap 内存堆结构体
 * @param size       要分配的内存大小
 *
 * @return 成功返回分配的内存地址
 */
void *Wed_memheap_malloc(struct Wed_memheap *p_mem_heap, unsigned long size){
    int                      ret;
    uint32_t                 free_size;
    struct Wed_memheap_item *p_header_ptr;

    if ((p_mem_heap == NULL) || (size == 0)) {
        return NULL;
    }

    /* 对齐分配大小 */
    size = WED_ALIGN(size, WED_MEMHEAP_ALIGN_SIZE);
    if (size < WED_MEMHEAP_MINIALLOC) {
        size = WED_MEMHEAP_MINIALLOC;
    }

    WED_DEBUG_PRINT("allocate %d on memheap 0x%x\r\n", size, (unsigned long)p_mem_heap);

    if (size < p_mem_heap->available_size) {
        free_size = 0;

        ret = Wed_mutex_lock(&p_mem_heap->lock, 5000);
        if (ret != 0) {
            return NULL;
        }

        /* 获取第一个空闲内存块 */
        p_header_ptr = p_mem_heap->p_free_list->p_next_free;

        while ((p_header_ptr != p_mem_heap->p_free_list) && (free_size < size)) {
            /* 获取当前内存块的大小 */
            free_size = WED_MEMITEM_SIZE_GET(p_header_ptr);
            if (free_size < size) {
                /* 内存块大小不满足申请需求，获取下一个空闲内存块 */
                p_header_ptr = p_header_ptr->p_next_free;
            }
        }

        /* 获取到适合的内存块 */
        if (free_size >= size) {
            /* 当前内存块可进行分割 */
            if (free_size >= (size + WED_MEMHEAP_ITEM_SIZE + WED_MEMHEAP_MINIALLOC)) {
                struct Wed_memheap_item *p_new_ptr;

                /* 分割内存块 */
                p_new_ptr = (struct Wed_memheap_item *)
                            (((uint8_t *)p_header_ptr) + size + WED_MEMHEAP_ITEM_SIZE);

                WED_DEBUG_PRINT("split mem block 0x%08x, next mem 0x%08x, prev mem 0x%08x, new mem 0x%08x\n",
                        p_header_ptr, p_header_ptr->p_next, p_header_ptr->p_prev, p_new_ptr);

                /* 初始化新的内存块条目 */
                p_new_ptr->magic     = WED_MEMHEAP_MAGIC;
                p_new_ptr->p_memheap = p_mem_heap;
                /* 插入内存块链表 */
                p_new_ptr->p_prev         = p_header_ptr;
                p_new_ptr->p_next         = p_header_ptr->p_next;
                p_new_ptr->p_next->p_prev = p_new_ptr;
                p_header_ptr->p_next      = p_new_ptr;

                /* 把分配的内存块从链表中移除 */
                p_header_ptr->p_next_free->p_prev_free = p_header_ptr->p_prev_free;
                p_header_ptr->p_prev_free->p_next_free = p_header_ptr->p_next_free;
                p_header_ptr->p_next_free = NULL;
                p_header_ptr->p_prev_free = NULL;

                /* 把新分割的内存块链入空闲链表 */
                p_new_ptr->p_next_free = p_mem_heap->p_free_list->p_next_free;
                p_new_ptr->p_prev_free = p_mem_heap->p_free_list;
                p_mem_heap->p_free_list->p_next_free->p_prev_free = p_new_ptr;
                p_mem_heap->p_free_list->p_next_free              = p_new_ptr;

                WED_DEBUG_PRINT("new ptr next free 0x%08x, prev free 0x%08x\r\n",
                        (unsigned long)p_new_ptr->p_next_free, (unsigned long)p_new_ptr->p_prev_free);

                /* 更新内存堆可用大小 */
                p_mem_heap->available_size = p_mem_heap->available_size - size - WED_MEMHEAP_ITEM_SIZE;
                /* 更新内存堆最大深度 */
                if ((p_mem_heap->mem_size - p_mem_heap->available_size) > p_mem_heap->max_used_size) {
                    p_mem_heap->max_used_size = p_mem_heap->mem_size - p_mem_heap->available_size;
                }
            } else {
                /* 更新内存堆可用大小 */
                p_mem_heap->available_size = p_mem_heap->available_size - free_size;
                /* 更新内存堆最大深度 */
                if ((p_mem_heap->mem_size - p_mem_heap->available_size) > p_mem_heap->max_used_size) {
                    p_mem_heap->max_used_size = p_mem_heap->mem_size - p_mem_heap->available_size;
                }

                /* 把分配的内存块从链表中移除 */
                p_header_ptr->p_next_free->p_prev_free = p_header_ptr->p_prev_free;
                p_header_ptr->p_prev_free->p_next_free = p_header_ptr->p_next_free;
                p_header_ptr->p_next_free = NULL;
                p_header_ptr->p_prev_free = NULL;
            }

            /* 标记为已被使用 */
            p_header_ptr->magic |= WED_MEMHEAP_ITEM_USED;

            Wed_mutex_unlock(&p_mem_heap->lock);

            WED_DEBUG_PRINT("alloc memory 0x%08x, memheap 0x%08x, size %d\r\n",
                    (unsigned long)((uint8_t *)p_header_ptr + WED_MEMHEAP_ITEM_SIZE),
                    (unsigned long)p_mem_heap,
                     size);

            return (void *)((uint8_t *)p_header_ptr + WED_MEMHEAP_ITEM_SIZE);
        }

        Wed_mutex_unlock(&p_mem_heap->lock);
    }

    WED_DEBUG_PRINT("allocate memory failed\r\n");

    return NULL;
}

//void rt_memheap_free(void *ptr)
//{
//    rt_err_t result;
//    struct rt_memheap *heap;
//    struct rt_memheap_item *header_ptr, *new_ptr;
//    rt_uint32_t insert_header;
//
//    /* NULL check */
//    if (ptr == RT_NULL) return;
//
//    /* set initial status as OK */
//    insert_header = 1;
//    new_ptr       = RT_NULL;
//    header_ptr    = (struct rt_memheap_item *)
//                    ((rt_uint8_t *)ptr - RT_MEMHEAP_SIZE);
//
//    RT_DEBUG_LOG(RT_DEBUG_MEMHEAP, ("free memory: memory[0x%08x], block[0x%08x]\n",
//                                    ptr, header_ptr));
//
//    /* check magic */
//    RT_ASSERT((header_ptr->magic & RT_MEMHEAP_MASK) == RT_MEMHEAP_MAGIC);
//    RT_ASSERT(header_ptr->magic & RT_MEMHEAP_USED);
//    /* check whether this block of memory has been over-written. */
//    RT_ASSERT((header_ptr->next->magic & RT_MEMHEAP_MASK) == RT_MEMHEAP_MAGIC);
//
//    /* get pool ptr */
//    heap = header_ptr->pool_ptr;
//
//    RT_ASSERT(heap);
//    RT_ASSERT(rt_object_get_type(&heap->parent) == RT_Object_Class_MemHeap);
//
//    /* lock memheap */
//    result = rt_sem_take(&(heap->lock), RT_WAITING_FOREVER);
//    if (result != RT_EOK)
//    {
//        rt_set_errno(result);
//
//        return ;
//    }
//
//    /* Mark the memory as available. */
//    header_ptr->magic &= ~RT_MEMHEAP_USED;
//    /* Adjust the available number of bytes. */
//    heap->available_size = heap->available_size + MEMITEM_SIZE(header_ptr);
//
//    /* Determine if the block can be merged with the previous neighbor. */
//    if (!RT_MEMHEAP_IS_USED(header_ptr->prev))
//    {
//        RT_DEBUG_LOG(RT_DEBUG_MEMHEAP, ("merge: left node 0x%08x\n",
//                                        header_ptr->prev));
//
//        /* adjust the available number of bytes. */
//        heap->available_size = heap->available_size + RT_MEMHEAP_SIZE;
//
//        /* yes, merge block with previous neighbor. */
//        (header_ptr->prev)->next = header_ptr->next;
//        (header_ptr->next)->prev = header_ptr->prev;
//
//        /* move header pointer to previous. */
//        header_ptr = header_ptr->prev;
//        /* don't insert header to free list */
//        insert_header = 0;
//    }
//
//    /* determine if the block can be merged with the next neighbor. */
//    if (!RT_MEMHEAP_IS_USED(header_ptr->next))
//    {
//        /* adjust the available number of bytes. */
//        heap->available_size = heap->available_size + RT_MEMHEAP_SIZE;
//
//        /* merge block with next neighbor. */
//        new_ptr = header_ptr->next;
//
//        RT_DEBUG_LOG(RT_DEBUG_MEMHEAP,
//                     ("merge: right node 0x%08x, next_free 0x%08x, prev_free 0x%08x\n",
//                      new_ptr, new_ptr->next_free, new_ptr->prev_free));
//
//        new_ptr->next->prev = header_ptr;
//        header_ptr->next    = new_ptr->next;
//
//        /* remove new ptr from free list */
//        new_ptr->next_free->prev_free = new_ptr->prev_free;
//        new_ptr->prev_free->next_free = new_ptr->next_free;
//    }
//
//    if (insert_header)
//    {
//        /* no left merge, insert to free list */
//        header_ptr->next_free = heap->free_list->next_free;
//        header_ptr->prev_free = heap->free_list;
//        heap->free_list->next_free->prev_free = header_ptr;
//        heap->free_list->next_free            = header_ptr;
//
//        RT_DEBUG_LOG(RT_DEBUG_MEMHEAP,
//                     ("insert to free list: next_free 0x%08x, prev_free 0x%08x\n",
//                      header_ptr->next_free, header_ptr->prev_free));
//    }
//
//    /* release lock */
//    rt_sem_release(&(heap->lock));
//}

